
// import oimo.dynamics.TimeStep;
// import oimo.m.Mat3;
// import oimo.m.Vec3;
// import oimo.m.M;
// import oimo.common.MathUtil;
// import oimo.common.Vec3;
// import oimo.dynamics.*;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.joint.JointSolverInfo;
// import oimo.dynamics.constraint.info.joint.JointSolverInfoRow;

import { Joint } from "./Joint";
import { SpringDamper } from "./SpringDamper";
import { RotationalLimitMotor } from "./RotationalLimitMotor";
import { JointBasis } from "./JointBasis";
import { Vec3 } from "../../../common/Vec3";

import { RevoluteJointConfig } from "./RevoluteJointConfig";
import { JointType } from "./JointType";
import { M } from "../../../common/M";
import { JointSolverInfo } from "../info/joint/JointSolverInfo";
import { TimeStep } from "../../TimeStep";
import { Mat3 } from "../../../common/Mat3";
import { JointSolverInfoRow } from "../info/joint/JointSolverInfoRow";
import { JacobianRow } from "../info/JacobianRow";
import { MathUtil } from "../../../common/MathUtil";

/**
 * A revolute joint (a.k.a. hinge joint) constrains two rigid bodies to share
 * their anchor points and constraint axes, and restricts relative rotation onto
 * the constraint axis. This joint provides one degree of freedom. You can enable
 * lower and upper limits, a motor, a spring and damper effect of the rotational
 * part of the constraint.
 */

class RevoluteJoint extends Joint {
	_sd: SpringDamper;
	_lm: RotationalLimitMotor;

	_basis: JointBasis;

	angle: number;
	angularErrorY: number;
	angularErrorZ: number;
	linearError: Vec3;

	/**
	 * Creates a new revolute joint by configuration `config`.
	 */
	constructor(config: RevoluteJointConfig) {
		super(config, JointType._REVOLUTE);

		M.vec3_fromVec3(this._localBasisX1, config.localAxis1);
		M.vec3_fromVec3(this._localBasisX2, config.localAxis2);

		this.buildLocalBasesFromX();

		this.angle = 0;
		this.angularErrorY = 0;
		this.angularErrorZ = 0;

		this._basis = new JointBasis(this);

		this._sd = config.springDamper.clone();
		this._lm = config.limitMotor.clone();
	}

	// --- ---

	public getInfo(info: JointSolverInfo, timeStep: TimeStep, isPositionPart: boolean): void {
		// compute ERP
		var erp: number = this.getErp(timeStep, isPositionPart);

		// compute rhs
		var linearRhs: Vec3 = new Vec3();
		M.vec3_scale(linearRhs, this.linearError, erp);
		var linRhsX: number = M.vec3_get(linearRhs, 0);
		var linRhsY: number = M.vec3_get(linearRhs, 1);
		var linRhsZ: number = M.vec3_get(linearRhs, 2);
		var angRhsY: number = this.angularErrorY * erp;
		var angRhsZ: number = this.angularErrorZ * erp;

		var crossR1: Mat3 = new Mat3();
		var crossR2: Mat3 = new Mat3();
		M.vec3_toCrossMatrix(crossR1, this._relativeAnchor1);
		M.vec3_toCrossMatrix(crossR2, this._relativeAnchor2);
		M.mat3_negate(crossR1, crossR1);
		M.mat3_negate(crossR2, crossR2);

		var row: JointSolverInfoRow;
		var j: JacobianRow;
		var motorMass: number = (this.computeEffectiveInertiaMoment(this._basis.x));

		// linear X
		row = info.addRow(this._impulses[0]);
		row.equalLimit(linRhsX, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 1, 0, 0);
		M.vec3_set(j.lin2, 1, 0, 0);
		M.mat3_getRow(j.ang1, crossR1, 0);
		M.mat3_getRow(j.ang2, crossR2, 0);

		// linear Y
		row = info.addRow(this._impulses[1]);
		row.equalLimit(linRhsY, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 0, 1, 0);
		M.vec3_set(j.lin2, 0, 1, 0);
		M.mat3_getRow(j.ang1, crossR1, 1);
		M.mat3_getRow(j.ang2, crossR2, 1);

		// linear Z
		row = info.addRow(this._impulses[2]);
		row.equalLimit(linRhsZ, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 0, 0, 1);
		M.vec3_set(j.lin2, 0, 0, 1);
		M.mat3_getRow(j.ang1, crossR1, 2);
		M.mat3_getRow(j.ang2, crossR2, 2);

		// angular X
		if (this._sd.frequency <= 0 || !isPositionPart) {
			row = info.addRow(this._impulses[3]);
			this.setSolverInfoRowAngular(row, this.angle, this._lm, motorMass, this._sd, timeStep, isPositionPart);

			j = row.jacobian;
			M.vec3_assign(j.ang1, this._basis.x);
			M.vec3_assign(j.ang2, this._basis.x);
		}

		// angular Y
		row = info.addRow(this._impulses[4]);
		row.equalLimit(angRhsY, 0);

		j = row.jacobian;
		M.vec3_assign(j.ang1, this._basis.y);
		M.vec3_assign(j.ang2, this._basis.y);

		// angular Z
		row = info.addRow(this._impulses[5]);
		row.equalLimit(angRhsZ, 0);

		j = row.jacobian;
		M.vec3_assign(j.ang1, this._basis.z);
		M.vec3_assign(j.ang2, this._basis.z);
	}


	private computeErrors(): void {
		var cos: number;

		// compute angular error along Y and Z
		var angError: Vec3 = new Vec3();
		M.vec3_cross(angError, this._basisX1, this._basisX2);
		cos = M.vec3_dot(this._basisX1, this._basisX2);
		var theta: number = MathUtil.safeAcos(cos);
		M.vec3_normalize(angError, angError);
		M.vec3_scale(angError, angError, theta);
		this.angularErrorY = M.vec3_dot(angError, this._basis.y);
		this.angularErrorZ = M.vec3_dot(angError, this._basis.z);

		// measure the rotation angle along X
		var perpCross: Vec3 = new Vec3();
		M.vec3_cross(perpCross, this._basisY1, this._basisY2);
		cos = M.vec3_dot(this._basisY1, this._basisY2);
		this.angle = MathUtil.safeAcos(cos);
		if (M.vec3_dot(perpCross, this._basis.x) < 0) {
			this.angle = -this.angle;
		}

		// compute linear error
		M.vec3_sub(this.linearError, this._anchor2, this._anchor1);
	}

	// --- internal ---

	public _syncAnchors(): void {
		super._syncAnchors();
		this._basis.trackByX();

		// compute positional errors
		this.computeErrors();
	}

	public _getVelocitySolverInfo(timeStep: TimeStep, info: JointSolverInfo): void {
		super._getVelocitySolverInfo(timeStep, info);
		this.getInfo(info, timeStep, false);
	}

	public _getPositionSolverInfo(info: JointSolverInfo): void {
		super._getPositionSolverInfo(info);
		this.getInfo(info, null, true);
	}

	// --- public ---

	/**
	 * Returns the first rigid body's constraint axis in world coordinates.
	 */
	public getAxis1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._basisX1);
		return v;
	}

	/**
	 * Returns the second rigid body's constraint axis in world coordinates.
	 */
	public getAxis2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._basisX2);
		return v;
	}

	/**
	 * Sets `axis` to the first rigid body's constraint axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAxis1To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._basisX1);
	}

	/**
	 * Sets `axis` to the second rigid body's constraint axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAxis2To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._basisX2);
	}

	/**
	 * Returns the first rigid body's constraint axis relative to the rigid body's transform.
	 */
	public getLocalAxis1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localBasisX1);
		return v;
	}

	/**
	 * Returns the second rigid body's constraint axis relative to the rigid body's transform.
	 */
	public getLocalAxis2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localBasisX2);
		return v;
	}

	/**
	 * Sets `axis` to the first rigid body's constraint axis relative to the rigid body's transform.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAxis1To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._localBasisX1);
	}

	/**
	 * Sets `axis` to the second rigid body's constraint axis relative to the rigid body's transform.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAxis2To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._localBasisX2);
	}

	/**
	 * Returns the rotational spring and damper settings.
	 */
	public getSpringDamper(): SpringDamper {
		return this._sd;
	}

	/**
	 * Returns the rotational limits and motor settings.
	 */
	public getLimitMotor(): RotationalLimitMotor {
		return this._lm;
	}

	/**
	 * Returns the rotation angle in radians.
	 */
	public getAngle(): number {
		return this.angle;
	}

}
